package com.rockwell.robot2015.views;

import java.text.DecimalFormat;
import java.util.List;

import org.opencv.android.BaseLoaderCallback;
import org.opencv.android.CameraBridgeViewBase.CvCameraViewFrame;
import org.opencv.android.LoaderCallbackInterface;
import org.opencv.android.OpenCVLoader;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.android.CameraBridgeViewBase;
import org.opencv.android.CameraBridgeViewBase.CvCameraViewListener2;
import org.opencv.imgproc.Imgproc;

import com.rockwell.robot2015.R;
import com.rockwell.robot2015.SettingsActivity;
import com.rockwell.robot2015.model.ColorBlobDetector;
import com.rockwell.robot2015.model.ColorBlobInfo;
import com.rockwell.robot2015.model.OnColorBlobDetectListener;
import com.rockwell.robot2015.model.OnOrientationChangeListener;

import android.content.Context;
import android.content.SharedPreferences;
import android.content.SharedPreferences.Editor;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.preference.PreferenceManager;
import android.util.AttributeSet;
import android.util.Log;
import android.view.LayoutInflater;
import android.view.MotionEvent;
import android.view.View;
import android.view.View.OnTouchListener;
import android.widget.RelativeLayout;
import android.widget.TextView;

public class MazeCameraView extends RelativeLayout implements OnTouchListener,
		CvCameraViewListener2 /* , SensorEventListener */{

	public enum TeachMode {
		Treasure, /*Obstacle,*/ Maze1, Maze2, Maze3, MazeEntranceGuide, OuterWall, DropArea
	}

	public interface OnTeachColorListener {
		void colorSelected(TeachMode mode);
	}
	
	private OnTeachColorListener m_teachColorListener;
	
	private static final String TAG = "MazeCameraView::RelativeLayout";

	private static final int DETECTOR_COUNT = 7;
	private static final int TREASURE_DETECTOR_INDEX = 0;
	//private static final int OBSTACLE_DETECTOR_INDEX = 1;
	private static final int MAZE1_DETECTOR_INDEX = 1;
	private static final int MAZE2_DETECTOR_INDEX = 2;
	private static final int MAZE3_DETECTOR_INDEX = 3;
	private static final int MAZE_ENTRANCE_DETECTOR_INDEX = 4;
	private static final int OUTERWALL_DETECTOR_INDEX = 5;
	private static final int DROPAREA_DETECTOR_INDEX = 6;

	private OnOrientationChangeListener m_OrientationListener;

	static class ColorDetector {
		public ColorBlobDetector detector;
		public ColorBlobInfo objectInfo;
		public OnColorBlobDetectListener listener;
	}

	/*
	 * private OnColorBlobDetectListener m_TreasureListener; private
	 * OnColorBlobDetectListener m_ObstacleListener; private
	 * OnColorBlobDetectListener m_maze1Listener; private
	 * OnColorBlobDetectListener m_maze2Listener; private
	 * OnColorBlobDetectListener m_maze3Listener; private
	 * OnColorBlobDetectListener m_mazeEntranceGuideListener; private
	 * OnColorBlobDetectListener m_outerWallListener; private
	 * OnColorBlobDetectListener m_DropAreaListener;
	 */

	private boolean m_TeachMode = false;
	private boolean m_IsColorSelected = false;
	private Mat m_Rgba;
	private Scalar m_BlobColorRgba;
	private Scalar m_BlobColorHsv;

	/*
	 * private ColorBlobDetector m_treasureDetector; private ColorBlobDetector
	 * m_obstacleDetector; private ColorBlobDetector m_maze1Detector; private
	 * ColorBlobDetector m_maze2Detector; private ColorBlobDetector
	 * m_maze3Detector; private ColorBlobDetector m_mazeEntranceGuideDetector;
	 * private ColorBlobDetector m_outerWallDetector; private ColorBlobDetector
	 * m_dropAreaDetector;
	 */

	private ColorDetector[] m_detectors = new ColorDetector[DETECTOR_COUNT];

	private int m_currIndex = 0;
	
	/*
	 * private ColorBlobInfo m_treasureInfo; private ColorBlobInfo
	 * m_obstacleInfo; private ColorBlobInfo m_maze1Info; private ColorBlobInfo
	 * m_maze2Info; private ColorBlobInfo m_maze3Info; private ColorBlobInfo
	 * m_mazeEntranceGuideInfo; private ColorBlobInfo m_outerWallInfo; private
	 * ColorBlobInfo m_DropAreaInfo;
	 */

	private Mat m_Spectrum;
	private Size SPECTRUM_SIZE;
	private Scalar CONTOUR_COLOR;

	private CameraBridgeViewBase m_OpenCvCameraView;
	private TextView m_CompassValueView;

	/*
	 * // Compass parameters private SensorManager m_SensorManager; private
	 * Sensor m_Gsensor; private Sensor m_Msensor; private float[] mGravity =
	 * new float[3]; private float[] mGeomagnetic = new float[3]; private float
	 * azimuth = 0f; private float currectAzimuth = 0; private float[]
	 * m_Orientation = new float[3]; private int m_Count;
	 */

	private TeachMode m_teachIndex = TeachMode.Treasure;

	private BaseLoaderCallback m_LoaderCallback = new BaseLoaderCallback(
			this.getContext()) {
		@Override
		public void onManagerConnected(int status) {
			switch (status) {
			case LoaderCallbackInterface.SUCCESS: {
				Log.i(TAG, "OpenCV loaded successfully");
				m_OpenCvCameraView.enableView();
			}
				break;
			default: {
				super.onManagerConnected(status);
			}
				break;
			}
		}
	};

	public MazeCameraView(Context context) {
		super(context);
		init(context);
	}

	public MazeCameraView(Context context, AttributeSet attrs) {
		super(context, attrs);
		init(context);
	}

	public MazeCameraView(Context context, AttributeSet attrs, int defStyle) {
		super(context, attrs, defStyle);
		init(context);
	}

	public void setOrientationChangeListener(OnOrientationChangeListener l) {
		m_OrientationListener = l;
	}

	public void setTreasureListener(OnColorBlobDetectListener l) {
		m_detectors[TREASURE_DETECTOR_INDEX].listener = l;
	}

	/*
	public void setObstacleListener(OnColorBlobDetectListener l) {
		m_detectors[OBSTACLE_DETECTOR_INDEX].listener = l;
	}
	*/
	
	public void setMaze1Listener(OnColorBlobDetectListener l) {
		m_detectors[MAZE1_DETECTOR_INDEX].listener = l;
	}

	public void setMaze2Listener(OnColorBlobDetectListener l) {
		m_detectors[MAZE2_DETECTOR_INDEX].listener = l;
	}

	public void setMaze3Listener(OnColorBlobDetectListener l) {
		m_detectors[MAZE3_DETECTOR_INDEX].listener = l;
	}

	public void setMazeEntranceGuideListener(OnColorBlobDetectListener l) {
		m_detectors[MAZE_ENTRANCE_DETECTOR_INDEX].listener = l;
	}

	public void setOuterWallListener(OnColorBlobDetectListener l) {
		m_detectors[OUTERWALL_DETECTOR_INDEX].listener = l;
	}

	public void setDropAreaListener(OnColorBlobDetectListener l) {
		m_detectors[DROPAREA_DETECTOR_INDEX].listener = l;
	}

	@Override
	protected void onLayout(boolean arg0, int arg1, int arg2, int arg3, int arg4) {
		super.onLayout(arg0, arg1, arg2, arg3, arg4);
	}

	/*
	 * @Override public void onAccuracyChanged(Sensor sensor, int accuracy) { }
	 * 
	 * @Override public void onSensorChanged(SensorEvent event) { final float
	 * alpha = 0.97f;
	 * 
	 * synchronized (this) { if (event.sensor.getType() ==
	 * Sensor.TYPE_ACCELEROMETER) { mGravity[0] = alpha * mGravity[0] + (1 -
	 * alpha) event.values[0]; mGravity[1] = alpha * mGravity[1] + (1 - alpha)
	 * event.values[1]; mGravity[2] = alpha * mGravity[2] + (1 - alpha)
	 * event.values[2]; }
	 * 
	 * if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
	 * mGeomagnetic[0] = alpha * mGeomagnetic[0] + (1 - alpha) event.values[0];
	 * mGeomagnetic[1] = alpha * mGeomagnetic[1] + (1 - alpha) event.values[1];
	 * mGeomagnetic[2] = alpha * mGeomagnetic[2] + (1 - alpha) event.values[2];
	 * }
	 * 
	 * float R[] = new float[9]; float I[] = new float[9]; boolean success =
	 * SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
	 * 
	 * if (success) { float[] data = new float[3];
	 * SensorManager.getOrientation(R, data);
	 * 
	 * data[0] = (float) Math.toDegrees(data[0]); data[0] = (data[0] + 360) %
	 * 360; data[1] = (float)Math.toDegrees(data[1]); data[2] =
	 * (float)Math.toDegrees(data[2]);
	 * 
	 * System.arraycopy(data, 0, m_Orientation, 0, 3);
	 * 
	 * DecimalFormat displayFormat = new DecimalFormat("#.#");
	 * 
	 * String sValues = "Azimuth:\t" +
	 * String.valueOf(displayFormat.format(m_Orientation[0])) + "\nPitch:\t" +
	 * String.valueOf(displayFormat.format(m_Orientation[1])) + "\nRoll:\t" +
	 * String.valueOf(displayFormat.format(m_Orientation[2]));
	 * 
	 * Log.d("Compass", sValues); //m_CompassValueView.setText(sValues);
	 * 
	 * if (m_OrientationListener != null)
	 * m_OrientationListener.onOrientationChanged(m_Orientation); } } }
	 */

	@Override
	public void onCameraViewStarted(int width, int height) {
		m_Rgba = new Mat(height, width, CvType.CV_8UC4);

		m_detectors[TREASURE_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(0, 255, 0));
		// TODO should be false.
		m_detectors[TREASURE_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(
				false);

		/*
		m_detectors[OBSTACLE_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(255, 255, 0));
		m_detectors[OBSTACLE_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(
				false);
		*/
		
		m_detectors[MAZE1_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(0, 255, 0));
		m_detectors[MAZE1_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(false); // true

		//m_detectors[MAZE1_DETECTOR_INDEX].detector.setColorRadius(new Scalar(25,50,50,0));
		m_detectors[MAZE1_DETECTOR_INDEX].detector.setColorRadius(new Scalar(12,25,25,0));

		m_detectors[MAZE2_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(0, 0, 255));
		m_detectors[MAZE2_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(false); // true

		//m_detectors[MAZE2_DETECTOR_INDEX].detector.setColorRadius(new Scalar(25,50,50,0));
		m_detectors[MAZE2_DETECTOR_INDEX].detector.setColorRadius(new Scalar(12,25,25,0));

		m_detectors[MAZE3_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(0, 255, 255));
		m_detectors[MAZE3_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(false); // true

		//m_detectors[MAZE3_DETECTOR_INDEX].detector.setColorRadius(new Scalar(25,50,50,0));
		m_detectors[MAZE3_DETECTOR_INDEX].detector.setColorRadius(new Scalar(12,25,25,0));

		m_detectors[MAZE_ENTRANCE_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(255, 255, 255));
		m_detectors[MAZE_ENTRANCE_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(
				false); // true

		m_detectors[OUTERWALL_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(255, 0, 0));
		m_detectors[OUTERWALL_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(
				true);

		m_detectors[DROPAREA_DETECTOR_INDEX].detector = new ColorBlobDetector(
				new Scalar(255, 0, 0));
		m_detectors[DROPAREA_DETECTOR_INDEX].objectInfo = new ColorBlobInfo(
				true);

		m_Spectrum = new Mat();
		m_BlobColorRgba = new Scalar(255);
		m_BlobColorHsv = new Scalar(255);
		SPECTRUM_SIZE = new Size(200, 64);
		CONTOUR_COLOR = new Scalar(255, 0, 0, 255);

		getTreasureColor();
		//getObstacleColor();
		getMaze1Color();
		getMaze2Color();
		getMaze3Color();
		getMazeEntranceGuideColor();
		getOuterWallColor();
		getDropAreaColor();

		m_IsColorSelected = true;
	}

	@Override
	public void onCameraViewStopped() {
		m_Rgba.release();
	}

	@Override
	public boolean onTouch(View v, MotionEvent event) {
		int cols = m_Rgba.cols();
		int rows = m_Rgba.rows();

		/*
		int xOffset = (m_OpenCvCameraView.getWidth() - cols) / 2;
		int yOffset = (m_OpenCvCameraView.getHeight() - rows) / 2;

		int x = (int) event.getX() - xOffset;
		int y = (int) event.getY() - yOffset;

		Log.i(TAG, "Touch image coordinates: (" + x + ", " + y + ")");

		if ((x < 0) || (y < 0) || (x > cols) || (y > rows))
			return false;

		Rect touchedRect = new Rect();

		touchedRect.x = (x > 4) ? x - 4 : 0;
		touchedRect.y = (y > 4) ? y - 4 : 0;

		touchedRect.width = (x + 4 < cols) ? x + 4 - touchedRect.x : cols
				- touchedRect.x;
		touchedRect.height = (y + 4 < rows) ? y + 4 - touchedRect.y : rows
				- touchedRect.y;
		*/

		// view center
		int cx = m_OpenCvCameraView.getWidth() / 2;
		int cy = m_OpenCvCameraView.getHeight() / 2;
		
		// distance to view center
		int dcx = (int)event.getX() - cx;
		int dcy = (int)event.getY() - cy;
		
		// coordinate in image
		int x = cols / 2 + dcy;
		int y = rows / 2 - dcx;
				
		Log.i(TAG, "Touch image coordinates: (" + x + ", " + y + ")");

		if ((x < 0) || (y < 0) || (x > cols) || (y > rows))
			return false;

		Rect touchedRect = new Rect();

		touchedRect.x = (x > 4) ? x - 4 : 0;
		touchedRect.y = (y > 4) ? y - 4 : 0;

		touchedRect.width = (x + 4 < cols) ? x + 4 - touchedRect.x : cols
				- touchedRect.x;
		touchedRect.height = (y + 4 < rows) ? y + 4 - touchedRect.y : rows
				- touchedRect.y;
		
		Mat touchedRegionRgba = m_Rgba.submat(touchedRect);

		Mat touchedRegionHsv = new Mat();
		Imgproc.cvtColor(touchedRegionRgba, touchedRegionHsv,
				Imgproc.COLOR_RGB2HSV_FULL);

		// Calculate average color of touched region
		m_BlobColorHsv = Core.sumElems(touchedRegionHsv);

		int pointCount = touchedRect.width * touchedRect.height;

		for (int i = 0; i < m_BlobColorHsv.val.length; i++)
			m_BlobColorHsv.val[i] /= pointCount;

		m_CompassValueView.setText("Set Treasure");

		TeachMode currIndex = m_teachIndex;
		
		switch (m_teachIndex) {
		case Treasure:
			//m_CompassValueView.setText("Set Obstacle");
			setTreasureColor();
			m_teachIndex = TeachMode.Maze1;
			break;

		/*
		case Obstacle:
			//m_CompassValueView.setText("Set Maze 1");
			setObstacleColor();
			m_teachIndex = TeachMode.Maze1;
			break;
		*/
			
		case Maze1:
			//m_CompassValueView.setText("Set Maze 2");
			setMaze1Color();
			m_teachIndex = TeachMode.Maze2;
			break;

		case Maze2:
			//m_CompassValueView.setText("Set Maze 3");
			setMaze2Color();
			m_teachIndex = TeachMode.Maze3;
			break;

		case Maze3:
			//m_CompassValueView.setText("Set Maze Entrance Guide");
			setMaze3Color();
			m_teachIndex = TeachMode.MazeEntranceGuide;
			break;

		case MazeEntranceGuide:
			//m_CompassValueView.setText("Set Outer Wall");
			setMazeEntranceGuideColor();
			m_teachIndex = TeachMode.OuterWall;
			break;

		case OuterWall:
			//m_CompassValueView.setText("Set Drop Area");
			setOuterWallColor();
			m_teachIndex = TeachMode.DropArea;
			break;

		case DropArea:
			//m_CompassValueView.setText("Set Treasure");
			setDropAreaColor();
			m_teachIndex = TeachMode.Treasure;
			break;
		}

		touchedRegionRgba.release();
		touchedRegionHsv.release();

		if (m_teachColorListener != null) {
			m_teachColorListener.colorSelected(currIndex);
		}
		
		return false; // don't need subsequent touch events
	}

	private void setDropAreaColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set drop area rgba color: (" + m_BlobColorRgba.val[0]
				+ ", " + m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2]
				+ ", " + m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[DROPAREA_DETECTOR_INDEX].detector;

		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setDropAreaColor(getContext(), m_BlobColorHsv);
	}

	private void setTreasureColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set treasure rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[TREASURE_DETECTOR_INDEX].detector;

		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setTreasureColor(getContext(), m_BlobColorHsv);
	}

	/*
	private void setObstacleColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set barrier rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[OBSTACLE_DETECTOR_INDEX].detector;

		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setObstacleColor(getContext(), m_BlobColorHsv);
	}
	*/
	
	private void setMaze1Color() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set barrier rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[MAZE1_DETECTOR_INDEX].detector;

		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setMaze1Color(getContext(), m_BlobColorHsv);
	}

	private void setMaze2Color() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set barrier rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[MAZE2_DETECTOR_INDEX].detector;

		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setMaze2Color(getContext(), m_BlobColorHsv);
	}

	private void setMaze3Color() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set barrier rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[MAZE3_DETECTOR_INDEX].detector;

		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setMaze3Color(getContext(), m_BlobColorHsv);
	}

	private void setMazeEntranceGuideColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set barrier rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[MAZE_ENTRANCE_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity
				.setMazeEntranceGuideColor(getContext(), m_BlobColorHsv);
	}

	private void setOuterWallColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "set outer wall rgba color: (" + m_BlobColorRgba.val[0]
				+ ", " + m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2]
				+ ", " + m_BlobColorRgba.val[3] + ")");

		ColorBlobDetector detector = m_detectors[OUTERWALL_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

		m_IsColorSelected = true;

		SettingsActivity.setOuterWallColor(getContext(), m_BlobColorHsv);
	}

	private void getTreasureColor() {
		m_BlobColorHsv = SettingsActivity.getTreasureColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get treasure rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[TREASURE_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum,
				SPECTRUM_SIZE);
	}

	/*
	private void getObstacleColor() {
		m_BlobColorHsv = SettingsActivity.getObstacleColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get barrier rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[OBSTACLE_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum,
				SPECTRUM_SIZE);
	}
	*/
	
	private void getMaze1Color() {
		m_BlobColorHsv = SettingsActivity.getMaze1Color(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get maze 1 rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[MAZE1_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);
	}

	private void getMaze2Color() {
		m_BlobColorHsv = SettingsActivity.getMaze2Color(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get maze 2 rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[MAZE2_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);
	}

	private void getMaze3Color() {
		m_BlobColorHsv = SettingsActivity.getMaze3Color(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get maze 3 rgba color: (" + m_BlobColorRgba.val[0] + ", "
				+ m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2] + ", "
				+ m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[MAZE3_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);
	}

	private void getMazeEntranceGuideColor() {
		m_BlobColorHsv = SettingsActivity
				.getMazeEntranceGuideColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get maze entrance guide rgba color: ("
				+ m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] + ", "
				+ m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[MAZE_ENTRANCE_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum,
				SPECTRUM_SIZE);
	}

	private void getOuterWallColor() {
		m_BlobColorHsv = SettingsActivity.getOuterWallColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get outer wall rgba color: (" + m_BlobColorRgba.val[0]
				+ ", " + m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2]
				+ ", " + m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[OUTERWALL_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum,
				SPECTRUM_SIZE);
	}

	private void getDropAreaColor() {
		m_BlobColorHsv = SettingsActivity.getDropAreaColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

		Log.i(TAG, "get drop area rgba color: (" + m_BlobColorRgba.val[0]
				+ ", " + m_BlobColorRgba.val[1] + ", " + m_BlobColorRgba.val[2]
				+ ", " + m_BlobColorRgba.val[3] + ")");

        ColorBlobDetector detector = m_detectors[DROPAREA_DETECTOR_INDEX].detector;
		detector.setHsvColor(m_BlobColorHsv);

		Imgproc.resize(detector.getSpectrum(), m_Spectrum,
				SPECTRUM_SIZE);
	}

	private void detect(ColorDetector detector) {
		detector.detector.process(m_Rgba);

		List<MatOfPoint> contours = detector.detector.getContours();
		// Log.i(TAG, "Barrier Contours count: " + contours.size());
		Imgproc.drawContours(m_Rgba, contours, -1, CONTOUR_COLOR);

		boolean present = detector.detector.detectShape(m_Rgba, detector.objectInfo);

		if (detector.listener != null)
			detector.listener.onDetected(present, detector.objectInfo, m_Rgba.height(), m_Rgba.width());
	}

	public void setTeachMode(boolean teachMode) {
		m_TeachMode = teachMode;

		if (m_TeachMode)
			m_OpenCvCameraView.setOnTouchListener(MazeCameraView.this);
		else
			m_OpenCvCameraView.setOnTouchListener(null);
	}

	public void setTeachItem(TeachMode mode) {
		m_teachIndex = mode;
		
		m_CompassValueView.setText("Set " + mode.toString());
	}
	
	public void setTeachColorListener(OnTeachColorListener listener) {
		m_teachColorListener = listener;
	}
	
	public Mat onCameraFrame(CvCameraViewFrame inputFrame) {
        m_Rgba = inputFrame.rgba();

    	if (m_currIndex >= DETECTOR_COUNT) {
    		m_currIndex = 0;
    	}

    	for (; m_currIndex < DETECTOR_COUNT; m_currIndex++) {
        	if (m_detectors[m_currIndex].listener.isEnabled()) {
        		detect(m_detectors[m_currIndex++]);
        		break;
        	}
    	}
        
        /*
        Mat colorLabel = m_Rgba.submat(4, 68, 4, 68);
        colorLabel.setTo(m_BlobColorRgba);

        Mat spectrumLabel = m_Rgba.submat(4, 4 + m_Spectrum.rows(), 70, 70 + m_Spectrum.cols());
        m_Spectrum.copyTo(spectrumLabel);
        */
        
        return m_Rgba;
    }

	private Scalar converScalarHsv2Rgba(Scalar hsvColor) {
		Mat pointMatRgba = new Mat();
		Mat pointMatHsv = new Mat(1, 1, CvType.CV_8UC3, hsvColor);
		Imgproc.cvtColor(pointMatHsv, pointMatRgba, Imgproc.COLOR_HSV2RGB_FULL,
				4);

		return new Scalar(pointMatRgba.get(0, 0));
	}

	private void init(Context context) {
		Log.i(TAG, "called init");

		LayoutInflater inflater = (LayoutInflater) context
				.getSystemService(Context.LAYOUT_INFLATER_SERVICE);
		inflater.inflate(R.layout.maze_camera_layout, this);

		m_CompassValueView = (TextView) findViewById(R.id.compass_values);
		m_OpenCvCameraView = (CameraBridgeViewBase) findViewById(R.id.opencv_view);
		m_OpenCvCameraView.setCvCameraViewListener(this);

		for (int i = 0; i < DETECTOR_COUNT; i++) {
			m_detectors[i] = new ColorDetector();
		}
		
		// m_SensorManager =
		// (SensorManager)getContext().getSystemService(Context.SENSOR_SERVICE);
		// m_Gsensor =
		// m_SensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		// m_Msensor =
		// m_SensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
	}

	public void onActivated() {
		OpenCVLoader.initAsync(OpenCVLoader.OPENCV_VERSION_2_4_7, getContext(),
				m_LoaderCallback);

		// m_SensorManager.registerListener(this, m_Gsensor,
		// SensorManager.SENSOR_DELAY_GAME);
		// m_SensorManager.registerListener(this, m_Msensor,
		// SensorManager.SENSOR_DELAY_GAME);
	}

	public void onDeactivated() {
		if (m_OpenCvCameraView != null)
			m_OpenCvCameraView.disableView();

		// m_SensorManager.unregisterListener(this);
	}
}
